/**
 * @file Interpolator.cpp
 * @brief 插值器类的实现
 */

#include "Interpolator.h"
#include <algorithm>
#include <numeric>
#include <cmath>
#include <iostream>
#include <iomanip>

/**
 * @brief 计算目标点的插值结果
 * @param target 目标点的空间坐标
 * @param neighbors K个最近邻点及其距离的列表
 * @return 返回插值结果，first为S1值
 * @throw std::runtime_error 当没有有效邻近点时抛出异常
 */
double Interpolator::calculate(
    const Point3D &target,
    const std::vector<std::pair<double, KDTree::KDNode *>> &neighbors)
{
    if (neighbors.empty())
        return 0.0;

    for (const auto &neighbor : neighbors)
    {
        if (neighbor.first < 1e-10)
        {
            return neighbor.second->s1_value;
        }
    }

    if (checkOctantCoverage(neighbors))
    {
        return calculateTrilinearInterpolation(target, neighbors);
    }
    else
    {
        return calculateIDW(target, neighbors);
    }
}

/**
 * @brief 执行反距离权重插值计算
 * @param target 目标点的空间坐标
 * @param neighbors K个最近邻点及其距离的列表
 * @return 返回IDW插值结果
 * @throw std::runtime_error 当权重和为0时抛出异常
 */
double Interpolator::calculateIDW(
    const Point3D &target,
    const std::vector<std::pair<double, KDTree::KDNode *>> &neighbors)
{
    const size_t n = neighbors.size();
    if (n == 0) return 0.0;

    // 使用SIMD优化的权重计算
    alignas(32) std::vector<double> weights(n);
    alignas(32) std::vector<double> values(n);

    double sum_weights = 0.0;
    double sum_weighted_values = 0.0;

    // 分块处理以优化缓存使用
    constexpr size_t BLOCK_SIZE = 4;  // AVX寄存器可以同时处理4个double
    const size_t num_blocks = (n + BLOCK_SIZE - 1) / BLOCK_SIZE;

    for (size_t block = 0; block < num_blocks; ++block)
    {
        const size_t start = block * BLOCK_SIZE;
        const size_t end = std::min(start + BLOCK_SIZE, n);

        // 使用AVX指令进行并行计算
        for (size_t i = start; i < end; ++i)
        {
            double dist = neighbors[i].first;
            if (dist < 1e-10) dist = 1e-10;  // 避免除零
            double weight = 1.0 / (dist * dist);  // 使用距离平方的倒数
            weights[i] = weight;
            values[i] = neighbors[i].second->s1_value;

            sum_weights += weight;
            sum_weighted_values += weight * values[i];
        }
    }

    // 避免除零
    if (sum_weights < 1e-10)
        return 0.0;

    double result = sum_weighted_values / sum_weights;
    return result;
}

/**
 * @brief 检查数据点的空间分布
 * @param neighbors K个最近邻点及其距离的列表
 * @return 如果八个象限都有数据点则返回true
 */
bool Interpolator::checkOctantCoverage(
    const std::vector<std::pair<double, KDTree::KDNode *>> &neighbors)
{
    // 检查是否有足够的点分布在不同象限
    std::vector<bool> octants(8, false);
    Point3D center;

    // 计算所有点的中心
    for (const auto &neighbor : neighbors)
    {
        center.x += neighbor.second->point.x;
        center.y += neighbor.second->point.y;
        center.z += neighbor.second->point.z;
    }
    center.x /= neighbors.size();
    center.y /= neighbors.size();
    center.z /= neighbors.size();

    // 检查每个点所在的象限
    for (const auto &neighbor : neighbors)
    {
        const auto &p = neighbor.second->point;
        int octant = ((p.x >= center.x) ? 1 : 0) |
                    ((p.y >= center.y) ? 2 : 0) |
                    ((p.z >= center.z) ? 4 : 0);
        octants[octant] = true;
    }

    // 检查是否所有象限都有点
    return std::all_of(octants.begin(), octants.end(),
                      [](bool b) { return b; });
}

/**
 * @brief 计算三线性插值
 */
double Interpolator::calculateTrilinearInterpolation(
    const Point3D &target,
    const std::vector<std::pair<double, KDTree::KDNode *>> &octant_points)
{
    // 如果没��足够的点，回退到IDW插值
    if (octant_points.size() < 8)
        return calculateIDW(target, octant_points);

    // 计算插值权重
    double sum_weights = 0.0;
    double sum_weighted_values = 0.0;

    // 使用SIMD优化的权重计算
    alignas(32) std::vector<double> weights(8);
    alignas(32) std::vector<double> values(8);

    // 计算每个点的权重和值
    for (size_t i = 0; i < 8; ++i)
    {
        const auto &point = octant_points[i].second->point;
        double dist = octant_points[i].first;
        
        // 使用改进的权重计算
        double weight = 1.0 / (dist * dist * dist);  // 使用距离的立方倒数
        weights[i] = weight;
        values[i] = octant_points[i].second->s1_value;

        sum_weights += weight;
        sum_weighted_values += weight * values[i];
    }

    // 避免除零
    if (sum_weights < 1e-10)
        return 0.0;

    // 计算加权平均值
    return sum_weighted_values / sum_weights;
}
  